In [1]:
import krpc

In [2]:
import numpy as np
import time

This is a simple launch. No reference to geo-position in control loop. Roll and yaw will be fixed at zero. Pitch will start at 90 deg until 60 m/s at which point we will pitch towards 30 deg above the eastern horizon at flame-out.

This is a booster-only single-body rocket with two stages: launch and parachute.


In [3]:
linkup = krpc.connect('192.168.1.2', name='First Flight')

In [4]:
import numpy as np
from matplotlib import pyplot, cm

inf = np.inf
isclose = np.isclose
π = np.pi
arctan = np.arctan
sign = np.sign

In [5]:
class Controller(object):
    '''Single Axis PID Controller'''
    def __init__(self,set_point=0,limits=(-inf,inf),kp=1,ki=0,kd=0,t0=0):
        # set point and control constants
        self.set_point = set_point
        self.min,self.max = limits
        self.kp = kp
        self.ki = ki
        self.kd = kd

        # time of previous call, running integral and
        # proportional term of previous call
        self.t0 = t0
        self.I  = 0
        self.P0 = 0

        # response value of previous call
        self.c = 0

    def __call__(self,x,t):
        # return previous value if no time has passed
        if isclose(t - self.t0, 0):
            return self.c

        # bring instance variables into local scope
        xset = self.set_point
        kp = self.kp
        ki = self.ki
        kd = self.kd

        # if parameters are all zero or None, return set point
        if not any([kp,ki,kd]):
            self.t0 = t
            return xset

        # bring instance variables into local scope
        t0 = self.t0
        I  = self.I
        P0 = self.P0

        # calculate PID terms
        Δt = t - t0
        P = xset - x
        ΔP = P - P0
        D = ΔP / Δt

        # freeze integral for a small time on
        # a large disturbance
        if self.ki > 0:
            if abs(kp*ΔP) > 0.5*(self.max - self.min):
                self._t0_freeze_I = t
            else:
                try:
                    if (t - self._t0_freeze_I) > self.ti:
                        del self._t0_freeze_I
                        I += P * Δt
                except AttributeError:
                    I += P * Δt

            # turn off integral term if kp*P is out of the
            # control range
            if not (self.min < kp*P < self.max):
                I = 0
            else:
                I = min(max(I, self.min/ki), self.max/ki)

        # clip proportional gain
        if not (self.min < kp*P < self.max):
            P = min(max(P, self.min/kp), self.max/kp)

        c = kp*P + ki*I + kd*D

        # clip output to specified limits
        c = min(max(c, self.min), self.max)

        # save parameters to class instance
        self.t0 = t
        self.I  = I
        self.P0 = P
        self.c  = c

        return c

    @property
    def ti(self):
        '''integral time'''
        return self.kp / self.ki
    @ti.setter
    def ti(self,ti):
        self.ki = self.kp / ti

    @property
    def td(self):
        '''derivative time'''
        return self.kd / self.kp
    @td.setter
    def td(self,td):
        self.kd = self.kp * td

    @property
    def ku(self):
        '''ultimate gain, assuming classic ziegler-nichols pid scheme'''
        return (1/.6)*self.kp
    @ku.setter
    def ku(self,ku):
        self.kp = .6*ku

    @property
    def tu(self):
        '''period of oscillation at ultimate gain'''
        return 2*self.kp/self.ki
    @tu.setter
    def tu(self,tu):
        self.ki = 2*self.kp/tu
        self.kd = self.kp*tu/8

    def ziegler_nichols(self,ku,tu,control_type='pid'):
        '''
            ku = ultimate gain
            tu = period of oscillation at ultimate gain
        '''
        converter = dict(
            p = lambda ku,tu: (.5*ku, 0, 0),
            pi = lambda ku,tu: (.45*ku, 1.2*(.45*ku)/tu, 0),
            pd = lambda ku,tu: (.8*ku, 0, (.8*ku)*tu/8),
            pid = lambda ku,tu: (.6*ku, 2*(.6*ku)/tu, (.6*ku)*tu/8),
            pessen = lambda ku,tu: (.7*ku, 2.5*(.7*ku)/tu, 3*(.7*ku)*tu/20),
            some_overshoot = lambda ku,tu: (.33*ku, 2*(.33*ku)/tu, (.33*ku)*tu/3),
            no_overshoot = lambda ku,tu: (.2*ku, 2*(.2*ku)/tu, (.2*ku)*tu/3)
        )
        self.kp,self.ki,self.kd = converter[control_type.lower()](ku,tu)

In [6]:
def main(linkup):
    ksc = linkup.space_center
    vessel = ksc.active_vessel
    
    body = vessel.orbit.body
    altitude = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'mean_altitude')
    vertical_speed = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'vertical_speed')
    speed = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'speed')
    pitch = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'pitch')
    heading = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'heading')
    roll = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'roll')
    
    con = vessel.control
    
    experiments = {'goo' : vessel.parts.with_name('GooExperiment')}
    capsule = vessel.parts.with_name('mk1pod')[0]
    
    def crew_report(capsule):
        report_module = capsule.module[2]
        report_action = report_module.actions[0]
        report_module.trigger_event(report_action)
    
    def observe_goo(goo_experiment):
        goo_module = goo_experiment.modules[1]
        observe_action = goo_module.actions[0]
        goo_module.trigger_event(observe_action)
    
    t0 = ksc.ut
    pitch_con = Controller(set_point=pitch()*π/180,limits=(-1,1),kp=1,ki=0.6,kd=1,t0=t0)
    heading_con = Controller(set_point=heading()*π/180,limits=(-1,1),kp=1,ki=0.6,kd=1,t0=t0)
    roll_con = Controller(set_point=roll()*π/180,limits=(-1,1),kp=1,ki=0.6,kd=1,t0=t0)
    
    observe_goo(experiments['goo'][0])
    time.sleep(3)
    
    con.activate_next_stage() # launch!
    
    while speed() < 60:
        t = ksc.ut
        con.yaw = pitch_con(pitch()*π/180, t)
        con.pitch = heading_con(heading()*π/180, t)
        con.roll = roll_con(roll()*π/180, t)
        time.sleep(0.001)
    
    ftot = vessel.resources.amount('SolidFuel')
    frem = ftot
    while frem > 0.1:
        frem = vessel.resources.amount('SolidFuel')
        pitch_con.set_point = 15 - (15 - 50) * (ftot - frem) / ftot
        t = ksc.ut
        con.yaw = pitch_con(pitch()*π/180, t)
        con.pitch = heading_con(heading()*π/180, t)
        con.roll = roll_con(roll()*π/180, t)
        time.sleep(0.001)
        
    reported = False
    while altitude() > 8000:
        if not reported and vertical_speed() < 0:
            observe_goo(experiments['goo'][1])
            crew_report(capsule)
            reported = True
        time.sleep(1)
            
    con.activate_next_stage() # parachutes
    
    while vessel.situation is not splashed:
        time.sleep(1)
    
    observe_goo(experiments['goo'][2])

In [7]:
main(linkup)


---------------------------------------------------------------------------
RPCError                                  Traceback (most recent call last)
<ipython-input-7-b30ba828d219> in <module>()
----> 1 main(linkup)

<ipython-input-6-9aeb249d1e5a> in main(linkup)
     38     while speed() < 60:
     39         t = ksc.ut
---> 40         con.yaw = pitch_con(pitch()*π/180, t)
     41         con.pitch = heading_con(heading()*π/180, t)
     42         con.roll = roll_con(roll()*π/180, t)

<string> in <lambda>(this, value)

/home/goetz/.local/lib/python3.4/site-packages/krpc/client.py in _invoke(self, service, procedure, args, kwargs, param_names, param_types, return_type)
     85         # Check for an error response
     86         if response.has_error:
---> 87             raise RPCError(response.error)
     88 
     89         # Decode the response and return the (optional) result

RPCError: Procedure not available in game scene 'Editor'

In [25]:
moi = np.array(vessel.moment_of_inertia)
itensor = np.array(vessel.inertia_tensor).reshape(3,3)
availtorque = np.array(vessel.available_torque)

print('''
moi: {moi}
itensor: {itensor}
availtorque: {availtorque}
'''.format(moi=moi, itensor=itensor, availtorque=availtorque))


moi: [ 1733.02355957   557.65820312  1732.93225098]
itensor: [[  1.73302319e+03   6.41541730e-04   1.02312282e-04]
 [  6.39679085e-04   5.57658203e+02   8.26553162e-03]
 [  1.03728089e-04   8.26367643e-03   1.73293176e+03]]
availtorque: [ 5000.  5000.  5000.]

Given a moment of intertia vector around (pitch, roll, yaw) in kg m^2 and an available torque vector around (pitch, roll, yaw) in N m or kg m^2 / s^2, I want to create a controller (presumably PID) in the range [-1,1] for each axis.


In [29]:
moment_of_inertia = 1733
max_torque = 5000
control_range = np.array([-1,1])

max_acceleration = max_torque * control_range[1] / moment_of_inertia
print('max accel:',max_acceleration,'rad / s^2')
print(max_acceleration*180/np.pi, 'deg / s^2')


max accel: 2.88517022504 rad / s^2
165.308077072 deg / s^2

This is very fast. I only want a couple degrees per second^2 acceleration and not more than about 5 deg/sec rotation ever, so let's see what control range that represents:


In [30]:
print('control range at 2 deg / s^2:',(2*np.pi/180) * moment_of_inertia / max_torque)


control range at 2 deg / s^2: 0.012098622374824692

This might be a good limit on the control rate of change.

The output would be for a device 2 deg from the set point at 2 deg/sec with zero windup: $$output = Kp * 2 + Kd * 2$$